//
//  TrackManager.m
//  NXTRemoteController
//
//  Created by naceka on 05.06.12.
//  Copyright 2012 __MyCompanyName__. All rights reserved.
//

#import "TrackManager.h"


@implementation TrackManager

- (TrackManager *) initWithRobot:(Robot *) r andView:(GestureView *) v {
	self = [super init];
	robot = r;
	view = v;
	motions_count = 0;
	timer = nil;
	speed = 75;
	timeForward = 1.0;
	timeLeft = 1.0;
	timeRight = 1.0;
	timeTurn = 1.0;
	return self;
}

- (void) computePath {
	//clean motions
	motions_count = 0;
	//get data
	CGPoint * points = [view getPoints];
	int count = [view getCount];
	int i = 0;
	
	//compute path only if min 1 segment exists
	if (count > 0) {
		enum MoveDirection last_direction = [self getDirectionOfSegmentFrom:&(points[0]) To:&(points[1])];
		int move_length = 0;
		
		switch (last_direction) {
			case MoveDirection_UP:
				//do nothing
				break;
			case MoveDirection_DOWN:
				[self addMotionWithLen:0 andDirection:MotionDirection_TURN];
				break;
			case MoveDirection_LEFT:
				[self addMotionWithLen:0 andDirection:MotionDirection_LEFT];
				break;
			case MoveDirection_RIGHT:
				[self addMotionWithLen:0 andDirection:MotionDirection_RIGHT];
				break;
			default:
				break;
		}
		
		//increase move length
		move_length++;
		
		i = 2;
		
		//
		while (i < count) {
			if ([self getDirectionOfSegmentFrom:&(points[i - 1]) To:&(points[i])] == last_direction) {
				//
				move_length++;
			}
			else {
				//save last motion	
				[self addMotionWithLen:move_length andDirection:MotionDirection_FORWARD];

				//turn
				enum MoveDirection current_direction = [self getDirectionOfSegmentFrom:&(points[i - 1]) To:&(points[i])];
				
				if (abs((int)current_direction - (int)last_direction) == 2) {
					//save turn
					[self addMotionWithLen:0 andDirection:MotionDirection_TURN];
				}
				else {
					enum MotionDirection md;
					//
					if (last_direction == MoveDirection_LEFT && current_direction == MoveDirection_UP 
						|| last_direction == MoveDirection_UP && current_direction == MoveDirection_RIGHT
						|| last_direction == MoveDirection_RIGHT && current_direction == MoveDirection_DOWN
						|| last_direction == MoveDirection_DOWN && current_direction == MoveDirection_LEFT) {
						md = MotionDirection_RIGHT;
					}
					else {
						md = MotionDirection_LEFT;
					}
					
					[self addMotionWithLen:0 andDirection:md];
				}
				
				//new move
				move_length = 1;
				last_direction = current_direction;
			}
			i++;
		}
		//save last motion
		[self addMotionWithLen:move_length andDirection:MotionDirection_FORWARD];
	}
}

- (enum MoveDirection) getDirectionOfSegmentFrom:(CGPoint *) a To:(CGPoint *) b {
	CGPoint r;
	r.x = b->x - a->x;
	r.y = b->y - a->y;
	enum MoveDirection d = MoveDirection_UNDEF;
	
	if (r.x == 100.0) {
		d = MoveDirection_RIGHT;
	}
	else if (r.x == -100.0) {
		d = MoveDirection_LEFT;
	} 
	else if (r.y == 100.0) {
		d = MoveDirection_DOWN;
	} else if (r.y == -100.0) {
		d = MoveDirection_UP;
	}
	
	return d;
}

- (void) addMotionWithLen:(int) l andDirection:(enum MotionDirection) d {
	motions[motions_count] = [[Motion alloc] initWithLength:l andDirection:d];
	motions_count++;
}

- (void) setTimeToForward:(float) f Left:(float) l Right:(float) r Turn:(float) t {
	timeForward = f;
	timeLeft = l;
	timeRight = r;
	timeTurn = t;
}

- (void) startRobotMotion {
	[self computePath];
	current_step = 0;
	[robot setSpeed:speed];
	[self nextStep];
}

- (void) nextStep {
	//while not end
	if (current_step < motions_count) {
		enum MotionDirection current_direction = [motions[current_step] getDirection];
		float time = 0.0;
		
		//check type and go
		switch (current_direction) {
			case MotionDirection_FORWARD:
				time = timeForward*[motions[current_step] getLength];
				[robot moveForward];
				break;
			case MotionDirection_LEFT:
				time = timeLeft;
				[robot moveLeft];
				break;
			case MotionDirection_RIGHT:
				time = timeRight;
				[robot moveRight];
				break;
			case MotionDirection_TURN:
				time = timeTurn;
				[robot moveLeft:speed andRight:-speed];
				break;
			default:
				break;
		}
		
		//timer on next step
		timer = [NSTimer scheduledTimerWithTimeInterval:time target:self selector:@selector(nextStep) userInfo:nil repeats:NO];
		current_step++;
	}
	else {
		//stop robot
		[robot stop];
		current_step = 0;
		timer = nil;
		
		//send message to delegate
		[delegate robotStopped];
	}

}

- (void) stopRobotMotion {
	current_step = 90000;
	timer = nil;
	[robot stop];
}

//delegate
- (void) setDelegate:(id <TrackManagerDelegate>) d {
	delegate = d;
}

@end
